#include "ActionController.h"
#define MAX_BALLOON 30

void analizeDegree(ArActionDesired *myDesired,double degree,double degreeRange);
int actionSee(IplImage *dest, Balloon *targets);
bool thereAreTarget(int num_target);
void getRange(ArRangeDevice *mySonar,double *centralRange,double *leftRange,double *rightRange,double radius);

/*
 This is the ActionTurn constructor, note the use of constructor chaining
 with the ArAction. also note how it uses setNextArgument, which makes
 it so that other things can see what parameters this action has, and
 set them.  It also initializes the classes variables.
 */
ActionController::ActionController(double stopDistance, double controlAngle,
		double velocityOnTarget, double velocityNearHurdle, double maxVelocity, ActionGo *go,ActionTurn *turn,ActionScouting *scout) :
	ArAction("Controller") {

	myStopDistance = stopDistance;
	myControlAngle = controlAngle;
	myVelocityOnTarget = velocityOnTarget;
	myVelocityNearHurdle=velocityNearHurdle;
	myMaxVelocity = maxVelocity;

	myGo = go;
	myTurn = turn;
	myScout=scout;

	connectToCam(1);
}

/*
 Sets the myRobot pointer (all setRobot overloaded functions must do this),
 finds the sonar device from the robot, and if the sonar isn't there,
 then it deactivates itself.
 //okk
 */
void ActionController::setRobot(ArRobot *robot) {
	ArAction::setRobot(robot);
	mySonar = robot->findRangeDevice("sonar");
	if (mySonar == NULL) {
		ArLog::log(ArLog::Terse,
				"actionExample: ActionTurn: Warning: I found no sonar, deactivating.");
		deactivate();
	}
}

/*
 This is the guts of the Turn action.
 */
ArActionDesired *ActionController::fire(ArActionDesired currentDesired) {

	/* Dichiarazione delle variabili  Visione*/
	IplImage *src;
	int num_target=0;
	Balloon targets[MAX_BALLOON];

	/* Dichiarazione delle variabili  Robot*/
	double centralRange,leftRange,rightRange;
	double radius =myRobot->getRobotRadius();
	bool target=false;
	double distCenter=200,distLeft=200,distRigh=200;
	double degree=0,degreeRange=60;
	int near=0;
	bool isY=false;
	double range;

	double delta=0;

	/* Acquisizione delle grandezze di interesse */
	myDesired.reset();

	/* Faccio un istantanea della scena */
	src = connectToCam(0);
	num_target=getTargets(src, targets,cvPoint(src->width / 2, src->height));
	/* Leggo i valori dei Sonar */
	range = mySonar->currentReadingPolar(-myControlAngle, myControlAngle)- myRobot->getRobotRadius();
	getRange(mySonar,&centralRange,&leftRange,&rightRange,radius);

	//printf("centrale =%f sinistra=%f destra=%f \n",centralRange,leftRange,rightRange);

	target=thereAreTarget(num_target);
	/*Individuazione stato in cui andare
	 * State=1 scoppia pallone
	 * State=2 evita ostacolo
	 * State=3 raggiungi taget
	 * State=4 esplora
	 * Quando vado nello stato 2, dobbiamo controllare i sensori in modo opportuno
	 * per capire se devo aggirare o meno un ostacolo per raggiungere il pallone giallo
	 * */

	if(range<myStopDistance){
		isY=isYellow(src,cvPoint(src->width / 2, src->height),144,128,40);
		//isY=false;
		if(isY && leftRange > distLeft && rightRange>distRigh)
			state=SCOPPIA_PALLONE;
		else   state=EVITA_OSTACOLO;
	}else {
			//se ho un obbiettivo lo raggiungo
			if(target)
				state=RAGGIUNGI_TARGET;
				//altrimenti vado in esplorazione
				else
					state=ESPLORA;
	}

	/*Esecuzione delle azioni opportune*/

	switch (state){
		case SCOPPIA_PALLONE: printf("State1: Scoppio Pallone \n");
								//disattivo azioni
								if(myTurn->isActive())
									myTurn->deactivate();
								if(myGo->isActive())
									myGo->deactivate();
								if(myScout->isActive())
									myScout->deactivate();

								myDesired.setVel(myVelocityOnTarget);
								break;
		case EVITA_OSTACOLO: printf("State2: Evita Ostacolo \n");

								if(myScout->isActive())
									myScout->deactivate();

//								if (myRobot->findAction("Go") == NULL)
									//myRobot->addAction(myGo,50);

								//myDesired.setVel(myVelocityNearHurdle);

								if(!myTurn->isActive())
									 myTurn->activate();

								if(num_target>0){
									// la turn dice che se -1 l'ostacolo è a sx e noi giriamo a dx
									// altrimenti 1
								   if(myTurn->myTurning<0)
										myScout->sign=-1;
									else myScout->sign=1;

								    myScout->angleLeft=90;
								    myScout->angleRight=360;
								}else {
									printf("turning %d\n\n",myTurn->myTurning);
									  if(myTurn->myTurning<0){
										  printf("sto girando dx\n\n");
										    myScout->sign=1;
										    myScout->angleLeft=0;
										    myScout->angleRight=360;
									  }else {
										  printf("sto girando sx\n\n");
											myScout->sign=-1;
										    myScout->angleLeft=360;
										    myScout->angleRight=0;

									  }

									    myScout->angleLeft=0;
										myScout->angleRight=180;



								}

								break;
		case RAGGIUNGI_TARGET: printf("State3: Raggiungi Target \n");

								near = nearest(targets, num_target);
								degree = targets[near].angle;

								//myDesired.setVel(myMaxVelocity);

								if(myTurn->isActive())
									myTurn->deactivate();

								if(myScout->isActive())
									myScout->deactivate();

								if(!myGo->isActive())
									myGo->activate();





								//if (myRobot->findAction("Go") == NULL)


								analizeDegree(&myDesired,degree,degreeRange);

								break;
		case ESPLORA: printf("State4: Esplora \n");

						if(myTurn->isActive())
							myTurn->deactivate();

						if (myGo->isActive())
							myGo->deactivate();

						if (!myScout->isActive()){
							myScout->startAngle=myRobot->getTh();
							myScout->angle=0;
							myScout->left=true;
							myScout->right=true;
							myScout->activate();
						}

						break;
	}

	return &myDesired;
}

/* Questa azione acquisice un frame dalla scena e restituisce l'immagine e il numero di
 * target presenti
 * */
bool thereAreTarget(int num_target){
	if(num_target>0)
		return true;
	else return false;
}
void getRange(ArRangeDevice *mySonar,double *centralRange,double *leftRange,double *rightRange,double radius){
	double centralAngle =10;
	double leftAngle1 =30,leftAngle2 =40;
	*centralRange=mySonar->currentReadingPolar(-centralAngle, centralAngle)- radius;
	*leftRange=mySonar->currentReadingPolar(leftAngle1, leftAngle2)- radius;
	*rightRange=mySonar->currentReadingPolar(-leftAngle2, -leftAngle1)- radius;

}
void analizeDegree(ArActionDesired *myDesired,double degree,double degreeRange){

	if (degree > degreeRange) {
		myDesired->setVel(0);
		myDesired->setDeltaHeading(-10);
	} else if (degree < -degreeRange) {
				myDesired->setVel(0);
				myDesired->setDeltaHeading(10);
			} else
				myDesired->setDeltaHeading(0);

}
